#include "SBSeparation.h"
#include "AIAgent.h"
#include "Timer.h"

SBSeparation::SBSeparation(AIAgent* agent, float distance) : Behaviour(agent), m_sqrSeparation(distance*distance)
{
	m_debugDrawColour = Vec3f(0.2f, 0.8f, 0.1f);
}

Vec3f SBSeparation::Update()
{
	//std::vector<AIAgent*>* neighbours = m_pAgent->GetNeighbours();
	std::vector<AIAgent*>::iterator it = m_pAgent->GetNeighbours()->begin();
	std::vector<AIAgent*>::iterator end = m_pAgent->GetNeighbours()->end();
	


	Vec3f force;
	Vec3f pos = m_pAgent->GetPos();
	for( ; it != end; ++it)
	{
		Vec3f deltaPos = pos - (*it)->GetPos();
		//float lenSqr = deltaPos.LengthSqr();

		if( deltaPos.LengthSqr() < m_sqrSeparation)
		{
			float dt = TheTimer::Instance()->GetDT();
			//force += deltaPos.NormaliseReturn() * 0.25f * dt; 
			force += deltaPos * m_pAgent->GetMaxForce();

		}
	}
	if(force == Vec3f())
	{
		return Vec3f();
	}
	else
	{
		force.Truncate(m_pAgent->GetMaxForce());
		return force;
	}
}